/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#pragma once

#include <cfloat>

#include <glog/logging.h>

#include "middleware/protocol/proto/v2xpb-asn-map.pb.h"
#include "middleware/protocol/proto/v2xpb-asn-message-frame.pb.h"
#include "middleware/protocol/proto/v2xpb-asn-rsi.pb.h"
#include "air_service/modules/perception-usecase/usecase/common/segment2d.h"
#include "air_service/modules/perception-usecase/usecase/common/polygon2d.h"

namespace os {
namespace v2x {
namespace protocol {
class RsiAffectPath {
 public:
  bool GetAffectPath(
      const airos::perception::usecase::Vec2d ev_point,
      const v2xpb::asn::Map& map,
      std::vector<v2xpb::asn::RsiReferencePath>* affect_paths) {
    if (map.nodes_size() <= 0) {
      LOG(WARNING) << "map node size <= 0";
      return false;
    }
    zone_ = map.nodes(0).position().xyz().zone();
    stop_points_.clear();
    if (!GetStopPoints(map, &stop_points_)) {
      LOG(WARNING) << "no stop point";
      return false;
    }
    if (IsInJunction(ev_point)) {
      LOG(INFO) << "calc in junction affect path";
      // 计算路口中心和所有link的影响路径
      return CalcAffectPath(map, affect_paths);
    }
    //  计算事件点所在link的影响路径
    v2xpb::asn::MapLink link;
    int lane_idx = 0;
    if (!GetNearstLink(ev_point, map, &link, &lane_idx)) {
      LOG(WARNING) << "can't find nearst link";
      return false;
    }
    v2xpb::asn::RsiReferencePath info;
    if (!CalcAffectPath(ev_point, link, lane_idx != 0, &info)) {
      LOG(WARNING) << "calc affect path failed";
      return false;
    }
    affect_paths->emplace_back(info);
    return affect_paths->size() > 0;
  }

 private:
  bool GetStopPoints(
      const v2xpb::asn::Map& map,
      std::vector<airos::perception::usecase::Vec2d>* points) {
    auto node = map.nodes(0);
    // get all stop point
    for (int i = 0; i < node.links_size(); ++i) {
      auto link = node.links(i);
      for (int j = 0; j < link.lanes_size(); ++j) {
        auto lane = link.lanes(j);
        int stop_point_idx = lane.positions_size() - 1;
        if (stop_point_idx < 0) {
          continue;
        }
        auto stop_point = lane.positions(stop_point_idx);
        points->emplace_back(stop_point.xyz().x(), stop_point.xyz().y());
      }
    }
    return points->size() > 0;
  }

  bool IsInJunction(
      const airos::perception::usecase::Vec2d point) {
    airos::perception::usecase::Polygon2d polygon(stop_points_);
    return polygon.IsPointIn(point);
  }

  bool CalcAffectPath(const v2xpb::asn::Map& map,
                      std::vector<v2xpb::asn::RsiReferencePath>* affect_paths) {
    auto node = map.nodes(0);
    auto center_pos = node.position().xyz();
    v2xpb::asn::RsiReferencePath info;
    if (!CalcAffectPath({center_pos.x(), center_pos.y()}, &info)) {
      return false;
    }
    affect_paths->emplace_back(info);
    for (int i = 0; i < node.links_size(); ++i) {
      auto link = node.links(i);
      airos::perception::usecase::Vec2d ev_point(-1, -1);
      v2xpb::asn::RsiReferencePath info;
      if (!CalcAffectPath(ev_point, link, true, &info)) {
        continue;
      }
      affect_paths->emplace_back(info);
    }
    return affect_paths->size() > 0;
  }

  bool CalcAffectPath(
      const airos::perception::usecase::Vec2d center_point,
      v2xpb::asn::RsiReferencePath* affect_path) {
    double max_len = 0;
    for (unsigned int i = 0; i < stop_points_.size(); ++i) {
      airos::perception::usecase::Segment2d line(
          center_point, stop_points_[i]);
      if (line.Length() > max_len) {
        max_len = line.Length();
      }
    }
    auto point_start = affect_path->add_points();
    point_start->mutable_xyz()->set_x(center_point.X());
    point_start->mutable_xyz()->set_y(center_point.Y());
    point_start->mutable_xyz()->set_z(0.0);
    point_start->mutable_xyz()->set_zone(zone_);
    auto point_end = affect_path->add_points();
    point_end->mutable_xyz()->set_x(center_point.X());
    point_end->mutable_xyz()->set_y(center_point.Y());
    point_end->mutable_xyz()->set_z(0.0);
    point_end->mutable_xyz()->set_zone(zone_);
    affect_path->set_radius(max_len);
    return true;
  }

  bool GetNearstLink(
      const airos::perception::usecase::Vec2d point,
      const v2xpb::asn::Map& map, v2xpb::asn::MapLink* ret_link, int* idx) {
    // calc min distance to point
    // return link pointer
    double min_len = DBL_MAX;
    bool is_find = false;
    auto& node = map.nodes(0);
    for (int i = 0; i < node.links_size(); ++i) {
      auto& link = node.links(i);
      for (int j = 0; j < link.lanes_size(); ++j) {
        auto& lane = link.lanes(j);
        for (int k = 0; k < lane.positions_size() - 1; ++k) {
          airos::perception::usecase::Vec2d s(
              lane.positions(k).xyz().x(), lane.positions(k).xyz().y());
          airos::perception::usecase::Vec2d e(
              lane.positions(k + 1).xyz().x(), lane.positions(k + 1).xyz().y());
          airos::perception::usecase::Segment2d line(s, e);
          double len = line.DistanceTo(point);
          if (len < min_len && len <= (lane.width() / 2)) {
            min_len = len;
            ret_link->CopyFrom(link);
            *idx = j;
            is_find = true;
          }
        }
      }
    }
    return is_find;
  }

  bool CalcAffectPath(
      const airos::perception::usecase::Vec2d ev_point,
      const v2xpb::asn::MapLink& link, bool ignore_left,
      v2xpb::asn::RsiReferencePath* affect_path) {
    LOG(INFO) << "link lane_size " << link.lanes_size() << ", link width "
              << link.width();
    // get affect path lane
    int affect_lane_idx = -1;
    double affect_path_radius = 0.0;
    if (link.lanes_size() % 2 == 0) {
      if (ignore_left) {
        LOG(INFO) << "ignore left lane";
        affect_lane_idx = link.lanes_size() / 2;
        affect_path_radius = link.width() / 2;
      } else {
        LOG(INFO) << "ignore right lane";
        affect_lane_idx = link.lanes_size() / 2 - 1;
        affect_path_radius = link.width() / 2;
      }
    } else {
      affect_lane_idx = link.lanes_size() / 2;
      affect_path_radius = link.width() / 2;
    }
    auto& lane = link.lanes(affect_lane_idx);
    // calc start point
    double start_x = 0.0;
    double start_y = 0.0;
    int next_idx = lane.positions_size() - 1;
    if (ev_point.X() < 0 && ev_point.Y() < 0) {
      LOG(INFO) << "calc affect path startwith stop point";
      auto stop_pos_idx = lane.positions_size() - 1;
      start_x = lane.positions(stop_pos_idx).xyz().x();
      start_y = lane.positions(stop_pos_idx).xyz().y();
    } else {
      LOG(INFO) << "calc affect path startwith ev point";
      airos::perception::usecase::Vec2d neatest_point;
      GetNearestPoint(lane, ev_point, &neatest_point, &next_idx);
      start_x = neatest_point.X();
      start_y = neatest_point.Y();
    }
    auto point_start = affect_path->add_points();
    point_start->mutable_xyz()->set_x(start_x);
    point_start->mutable_xyz()->set_y(start_y);
    point_start->mutable_xyz()->set_z(0.0);
    point_start->mutable_xyz()->set_zone(zone_);
    // calc 100m pointer
    double affect_path_len = 0;
    airos::perception::usecase::Vec2d s(start_x, start_y);
    airos::perception::usecase::Vec2d e(
        lane.positions(next_idx).xyz().x(), lane.positions(next_idx).xyz().y());
    airos::perception::usecase::Segment2d line(s, e);
    affect_path_len += line.Length();
    for (int i = next_idx; i > 0; --i) {
      airos::perception::usecase::Vec2d s(
          link.positions(i).xyz().x(), link.positions(i).xyz().y());
      airos::perception::usecase::Vec2d e(
          lane.positions(i - 1).xyz().x(), lane.positions(i - 1).xyz().y());
      airos::perception::usecase::Segment2d line(s, e);
      affect_path_len += line.Length();
      if (affect_path_len >= 100 || i - 1 == 0) {
        auto point_end = affect_path->add_points();
        point_end->mutable_xyz()->set_x(e.X());
        point_end->mutable_xyz()->set_y(e.Y());
        point_end->mutable_xyz()->set_z(0.0);
        point_end->mutable_xyz()->set_zone(zone_);
        break;
      }
    }
    affect_path->set_radius(affect_path_radius);
    return true;
  }

  bool GetNearestPoint(
      const v2xpb::asn::MapLane& lane,
      const airos::perception::usecase::Vec2d ev_point,
      airos::perception::usecase::Vec2d* neatest_point,
      int* next_idx) {
    double min_len = DBL_MAX;
    for (int i = lane.positions_size() - 1; i > 0; --i) {
      airos::perception::usecase::Vec2d s(
          lane.positions(i).xyz().x(), lane.positions(i).xyz().y());
      airos::perception::usecase::Vec2d e(
          lane.positions(i - 1).xyz().x(), lane.positions(i - 1).xyz().y());
      airos::perception::usecase::Segment2d line(s, e);
      airos::perception::usecase::Vec2d tmp;
      double len = line.DistanceTo(ev_point, &tmp);
      if (len < min_len) {
        neatest_point->SetX(tmp.X());
        neatest_point->SetY(tmp.Y());
        min_len = len;
        *next_idx = i - 1;
      }
    }
    return true;
  }

 private:
  int zone_;
  std::vector<airos::perception::usecase::Vec2d> stop_points_;
};

}  // namespace protocol
}  // namespace v2x
}  // namespace os
